Controlling a robot based on constraint-consistent and sequence-optimized pose adaptation

ABSTRACT

A method for controlling at least one effector trajectory of an effector of a robot for solving a predefined task is proposed. A sequence of postures are acquired to modify at least one of a contact constraint topology and an object constraint topology. A set of constraint equations are generated based on at least one of the modified contact constraint topology and the modified object constraint topology. On the generated set of constraint equations, a constraint relaxation is performed to generate a task description including a set of relaxed constraint equations. The at least one effector trajectory is generated by applying a trajectory generation algorithm on the task description. An inverse kinematics algorithm is performed to generate a control signal from the at least one effector trajectory. At least one effector is controlled to execute the at least one effector trajectory based on the generated control signal.

TECHNICAL FIELD

The invention relates to the field of motion planning in robotics, inparticular multi-contact motion planning methods and correspondingmotion planning systems.

BACKGROUND

Multi-contact motion planning may be described as the problem of findinga contact sequence and a robot motion for performing movement tasks withchanging contacts between a robot and physical structures in theenvironment. Movement tasks may include walking gait patterns for mobilerobots. Further movement tasks may include manipulating physicalobjects.

Walking gait patterns for mobile robots include a sequence of contactsbetween ground surfaces in the environment, and movement means (“feet”)of the mobile robot. For determining a walking gait pattern, a sequenceof contacts between the feet of the robot and the ground surface needsto be determined.

For manipulating a physical object in the environment of the robot, asequence of contacts of at least one end-effector (“manipulator”,“hand”, “finger”) of the robot with the physical object has to bedetermined.

Determining solutions for problems of multi-contact motion planningrequires ample computation resources. Presently, there exists no optimalsolution for addressing tasks in the field of multi-contact motionplanning. A known strategy for addressing such tasks includes breakingdown the task into subtasks, and subsequently solving each sub-taskseparately.

The publication “Planning support contact-points for acyclic motions andexperiments on HRP-2”, by Adrien Escande, Abderahmane Kheddar, SylvainMiossec, and Sylvain Garsault, in: Experimental Robotics. Springer,Berlin, Heidelberg, 2009, discloses an approach for contact beforemotion planning for humanoid robots that enables addressing the task offinding a path for a mobile robot in a constrained environment.

According to the known strategy for addressing such tasks, the problemof multi-contact motion planning may be divided into subtasks of

-   -   searching for contact candidates (step 1);    -   a geometric analysis of the operating space of the robot may be        used for generating the contact candidates for a robot        multi-contact locomotion task;    -   searching a contact sequence from the contact candidates of step        1 and estimating corresponding contact poses (step 2);    -   a potential-field based approach may be used to search for the        sequence of contacts and for estimating the corresponding        contact poses (robot poses);    -   optimizing a trajectory between the contact poses of step 2        (step 3);    -   the trajectory optimization connects the contacts of the contact        sequence by connecting the corresponding contact poses of the        robot.

Separating the problem into subtasks may results in a computationallymore manageable problem. Nevertheless, a major drawback of this approachis the loss of the interdependencies between the individual computationsteps: the search for the contact sequence and pose estimation of asubsequent second step base on the results of the preceding first step.Consequentially, in the second step, the contact candidates of the firststep are not adapted in the second step. Similarly, optimal trajectoriesdetermined in a third step connect the contact poses estimated in thesecond step, and do only limited adaptation of locations of the contactsof the contact sequence, if any at all. Adapting of contact poses for agiven contact between the third step and the second step are typicallyperformed using techniques and algorithms known as Inverse Kinematics inthe art.

Presently, motion planning for sequential tasks is a challengingsubject. One of the key aspects is to determine a feasible contactsituation between the robot and a physical structure such as a physicalobject (object) and/or ground surface in the environment of the robot,so that successive steps in the sequence of postures of the robot do notexceed physical limits of the robot. In many cases, an interdependencebetween the successive steps of the sequence exists. For example,considering a sequence of postures which initially grasps a large objectusing a comfortable pose may result subsequently in inconvenient poses,for instance, when the task of moving the object and simultaneouslyturning the object while continuously maintaining contact betweenmanipulators of the robot and the object is executed.

The problem of determining contacts and object poses in a sequentialtask of multi-contact motion planning in an efficient manner whileovercoming the problems discussed above requires addressing.

SUMMARY

A multi-contact motion planning method according to the first aspect, anon-transitory storage medium storing program instructions according toa second aspect and a corresponding motion planning system according toa third aspect address the problem.

A method for controlling at least one effector trajectory of an effectorof a robot for solving a predefined task according to a first aspectstarts with acquiring an (initial) sequence of postures in a first step.The acquired sequence of postures may solve a predefined task. Eachposture of the sequence of postures includes at least one contact pointand a kinematic pose of the at least one effector. The method proceedsby modifying a contact constraint topology according to the acquiredsequence of postures; additionally or alternatively, an objectconstraint topology according to the acquired sequence of postures ismodified according to the acquired sequence of postures. The method thengenerates a set of constraint equations (model, kinematic model) basedon at least one of the modified contact constraint topology and themodified object constraint topology. On the generated set of constraintequations, a constraint relaxation is performed in order to generate atask description including a set of relaxed constraint equations. Themethod proceeds by generating the at least one effector trajectory byapplying a trajectory generation algorithm on the task description. Aninverse kinematics algorithm is then performed to generate a controlsignal from the at least one effector trajectory. The method proceeds bycontrolling the at least one effector to execute the at least oneeffector trajectory based on the generated control signal.

A non-transitory computer-readable storage medium according to a secondaspect stores a program of machine-readable instructions executable by adigital processing apparatus to cause the digital processing apparatusto perform the method according to the first aspect.

A system (robotic system) for controlling at least one effectortrajectory of a robot for solving a predefined task according to a thirdaspect comprises an acquisition unit configured to acquire a sequence ofpostures, wherein each posture includes at least one contact point and akinematic pose of the at least one effector. The system furthercomprises a processor configured to modify at least one of a contactconstraint topology according to the acquired sequence of postures, andan object constraint topology according to the acquired sequence ofpostures. The processor is further configured to generate a set ofconstraint equations based on the at least one of the modified contactconstraint topology and the modified object constraint topology, toperform a relaxation on the generated set of constraint equations togenerate a task description including the set of relaxed constraintequations, to generate at least one effector trajectory by applying atrajectory generation algorithm on the set of constraint equations, toperform an inverse kinematics algorithm on the generated at least oneeffector trajectory for generating a control signal for controlling theat least one effector. The processor generates and outputs to the robota control signal. The control signal is adapted to control the at leastone effector to execute the generated at least one effector trajectory.The robot controls the at least one effector based on the controlsignal.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 illustrates a robot-object manipulation scenario in an exemplaryapplication of an embodiment.

FIG. 2 provides an overview of a robotic system and its main structuralelements according to an embodiment.

FIG. 3 illustrates an object-contact-model and a robot model for therobot-object manipulation scenario in the exemplary application of FIG.1A.

FIG. 4 depicts a method for controlling a robot applying an embodimentof the inventive method for motion planning in a flowchart.

FIG. 5 illustrates the method for controlling an effector trajectory ofa robot according to an embodiment in a flowchart.

FIG. 6 depicts a flowchart for a process of connecting constraintsacross the sequence of postures in an implementation of the invention.

FIG. 7 depicts a flowchart for a process of connecting objects acrossthe sequence of postures in an implementation of the invention.

FIG. 8A illustrates a robot-object manipulation scenario including asequence of steps in an exemplary application of an embodiment.

FIG. 8B shows a sequence model of the manipulation scenario according toFIG. 4A comprising a sequence of postures without coupling ofconsecutive steps.

FIG. 8C shows a sequence model of the manipulation scenario according toFIG. 4A comprising a sequence of postures with coupling of fixedcontacts of the consecutive steps.

FIG. 8D shows a sequence model of the manipulation scenario according toFIG. 4A comprising a sequence of postures with coupling of fixedcontacts of the consecutive steps and coupling of constant object poses.

FIG. 9 illustrates a graph representation of the task comprising twoalternative branches in an implementation of the invention. and

FIG. 10 illustrates a comparison for a sequence of postures before andafter an optimization of the sequence of postures.

Same or corresponding features are denoted in the figures by samereference signs. The description uses terms known in the art ofrobotics. For convenience, some short references are provided beforediscussing examples of the invention in more detail with reference tothe drawings.

DETAILED DESCRIPTION

The term “constraint” refers to a particular constraint condition in thesequence of postures. The sequence of postures is modelled as amultibody system. The constraint condition implies a restriction in thekinematic degrees of freedom of one or more bodies. The degrees offreedom denote the number of independent kinematic possibilities ofmoving, for example, degrees of freedom correspond to the minimum numberof parameters required to define the position of the object in space. Abody is a rigid or flexible part of a mechanical system, for example,the effector of robot. A link is the connection of two or more bodies,or a body with the ground. The link is defined by kinematicalconstraints that restrict the relative motion of the bodies. Theconstraint condition may be an algebraic equation that defines therelative translation or rotation between two bodies. In the particularcase of the contact constraint, the relative translation is zero.

An original set of constraint equations comprises the contact constrainttopology, the object constraint topology and task constraints. Thegenerated set of constraint equations, which is generated based on atleast one of the modified contact constraint topology and the modifiedobject constraint topology and after performing constraint relaxationcan have either a same size as the original set of constraint equations,or a reduced size compared to the original set of constraint equations.The same size applies in a case, in which constraints of a currentposture are changed to refer to the previous posture, but a dimension ofthe constraints is the same. The reduced size applies in a case, inwhich constraints of a current posture are removed during performingconstraint relaxation.

The method according to the first aspect determines robot-object orrobot-environment contacts while simultaneously taking an overallsequence of postures (steps) into account. The method performs acomputationally efficient contact pose adaptation for the overallcontact sequence while regarding an overall optimality criterion.Comparing the method with the known approach discussed above, theapproach adds an additional computation step, such that the method reads

-   -   contact before motion planning (step 1)    -   searching a contact sequence and estimating corresponding        contact pose (step 2)    -   adapting sequence-optimal contact pose (step 2A)    -   optimizing a trajectory between adapted contact poses (step 3)

The new step 2A allows to compute robot motions that will not be foundusing the state-of-the art approach that includes corresponding steps(step 1), (step 2), and (step 3). Consequentially, motions of the robothaving a higher quality may be determined. Motions of a higher qualitymay include, for example, motions of effectors of the robot that showincreased distance to joint range limits or reliably avoid collisions.The approach according to the method eases the use of a planningalgorithm in step 1, since a user does not need to fine-tune a contactsituation in more detail for motion planning. Further, the method mayprovide an optimized trajectory faster when compared with traditionaltrajectory optimization algorithms, in some cases even some orders ofmagnitudes faster.

Currently, no concept to perform pose adaptation under consideration ofan overall sequence of postures exists.

The method enables determining a sequence of optimal robot poses andobject poses for tasks that comprise a sequence of object handlingsteps. The object may move between individual steps. The method can alsodetermine sequences of postures implementing object handling steps,which include decision steps for deciding between alternate options howto proceed in the sequence. These sequences including one or moredecision steps may be formulated as graphs of postures. The method isapplicable for different types of graphs of postures, for instancehaving a tree structure with alternate branches, sequences of posturesshowing closed chains of posture sequences, or having directed acyclicgraph structures. The method enables adjusting object constraints andcontact constraints to optimize a global criteria function, whilemaintaining constraints consistent that do not change in the sequence ata consistent fixed pose. The method allows an implementation of aplurality of robot handling scenarios, ranging from assembly tasks tohuman-robot collaborative tasks in a computationally efficient manner.

Contrary thereto, known methods such as trajectory optimization (TO) canonly deal with linear sequences, and not with a particular class ofsequences of postures that comprise decision points at which thesequence branches into two or more alternative options when proceedingfurther. This particular class of sequences of postures may be called agraph of postures or a tree of postures. Furthermore, known trajectoryoptimization requires more specifications for formulating the task: atrajectory representation, time-points, and other details such ascriteria functions that determine a motion behavior when transitioningfrom one step to another step in the sequence of postures.

Contrary thereto, the proposed method is computationally more efficientand requires less specifications for describing the task. In addition,the method may be applied in a plurality of use cases that currentlycannot be realized with known trajectory optimization methods. Forexample, the method may be advantageously employed when consideringseveral options for addressing the task, or in an interactive simulationhow to approach the task using a user interface, in particular agraphical user interface (GUI).

The method is not limited to a particular contact sequence domain. Themethod may, for example, be applied to walking gait patterns of mobilerobots, or to object manipulation sequences and similar tasks in thewide field of robotics.

The method for controlling at least one effector trajectory of aneffector of a robot for solving a predefined task according to a firstaspect starts with acquiring an (initial) sequence of postures in afirst step. The acquired sequence of postures may solve a predefinedtask. Each posture of the sequence of postures includes at least onecontact point and a kinematic pose of the at least one effector. Themethod proceeds by modifying a contact constraint topology according tothe acquired sequence of postures; additionally or alternatively, anobject constraint topology according to the acquired sequence ofpostures is modified according to the acquired sequence of postures. Themethod then generates a set of constraint equations (model) based on atleast one of the modified contact constraint topology and the modifiedobject constraint topology. On the constraints, a relaxation isperformed in order to generate a task description including the set ofrelaxed constraint equations. The method proceeds by generating the atleast one effector trajectory by applying a trajectory generationalgorithm on the task description. An inverse kinematics algorithm isthen performed to generate a control signal from the at least oneeffector trajectory. The method proceeds by controlling the at least oneeffector to execute the at least one effector trajectory based on thegenerated control signal.

The method according to a preferred embodiment comprises in the step ofmodifying the contact constraint topology, in case a contact location inthe object frame of a current posture in the sequence of posturesremains the same as a contact location of the corresponding contact inan immediately preceding posture of the sequence of postures, removing aconstraint of the contact to the object in the current posture, andconnecting a new constraint of the contact in the current posture to thecorresponding contact in at least one immediately preceding posture ofthe sequence of postures.

According to a preferred embodiment, the method includes the step ofmodifying the object constraint topology, which comprises, in case anobject pose of the current posture in the sequence of postures remainsthe same as the object pose in an immediately preceding posture of thesequence of postures, removing a constraint of the object in the currentposture, and connecting a new constraint of the object in the currentposture to the corresponding object in at least one immediatelypreceding posture of the sequence of postures. For example, the step ofremoving a constraint of the object in the current posture removes aconstraint with respect to a reference frame.

The method according to an advantageous embodiment comprises thesequence of postures as a linear sequence, which arranges all individualpostures of the sequence in a time-sequential manner. Additionally oralternatively, the sequence of postures has a graph structure modellingat least two alternate options for performing the task by controllingthe at least one effector trajectory. The graph structure may have, forinstance, a tree structure (trees of postures), a closed chainstructure, or a directed acyclic graph structure. A closed chain ofpostures is a graph structure in which a last posture of the sequence ofpostures connects to a first posture of the sequence of postures. Thesequence of postures having a graph structure includes at least twoindividual postures, which the sequence arranges for execution at acorresponding point in time. The at least two individual postures, whichthe sequence arranges for execution at a corresponding point in timerepresent postures to be performed alternatively for performing thepredefined task.

According to an embodiment of the method for controlling at least oneeffector trajectory, performing constraint relaxation on the generatedset of constraint equations comprises at least one of regularizing atleast one individual constraint by allowing a deviation from theindividual constraint, removing at least one constraint which isinvariant for the task, and removing at least one constraint in case avalue of the constraint coordinate is within a predetermined interval.

In the method according to an embodiment, each contact point is modelledin a kinematic chain via an object coordinate frame of an object in theenvironment. Each contact, may, for example be modelled in the kinematicchain to a reference frame in the environment of the robot.

Each (individual) posture of the acquired sequence of postures may beindependent of other postures of the acquired sequence of postures.

The method for controlling at least one effector trajectory may controlthe effector in real-time.

The method according to an advantageous embodiment comprises further astep of adjusting the effector trajectory of a robot to match a qualityfunction. The quality function includes a description of a humanergonomic state determined based on a given ergonomic model of a humanacting as a collaborative partner of the robot (collaborating with therobot in order to address the predefined task). The adjusted effectortrajectory defines an effector-and-object pose of the robot.

According to an embodiment of the method for controlling at least oneeffector trajectory, the method further comprises a step of determiningan optimized location of the robot for performing the task.

The method according to an advantageous embodiment comprises performingthe method for solving a predetermined task with each of at least twodifferent robots. The method proceeds by executing an additional step ofdetermining, which of the at least two different robots is more suitableby comparing quality criteria for performing the task by each of the atleast two different robots.

According to an embodiment of the method for controlling at least oneeffector trajectory, in the step of performing constraint relaxation onthe generated set of constraint equations, at least one constraint in aconstraint equation on at least one location of a tool or an objectrequired for solving the predefined task is relaxed, and the methodfurther comprises a step of determining at least one optimized locationof the tool or the object for designing a workplace within anenvironment for performing the task.

The method may be executed as part of an online receding-horizonplanning method.

The method for controlling at least one effector trajectory according afurther embodiment generates the at least one effector trajectory asoptimal with respect to one of the at least two alternate options.Alternatively the method generates the effector trajectory as optimalwith respect to a weighted average of the two alternate options.

According to an embodiment, the method for controlling at least oneeffector trajectory comprises acquiring a plurality of sequences ofpostures for performing the predefined task, wherein individualsequences of postures of the acquired plurality of sequences of posturesdiffer by the number of individual postures included in the respectivesequence of postures. The method performs the method for each individualsequence of postures of the acquired plurality of sequences of postures.The method further discards individual sequences of postures of theplurality of postures, which result in a corresponding effectortrajectory that violates at least one motion limit of the robot.

The term configuration refers to a minimal coordinate representation ofthe kinematics of the robot and of the physical object for a singlepoint in time.

The term posture refers to a particular kinematic arrangement of a setor a tree of rigid bodies according to a given configuration.

A sequence of postures denotes a time-ordered sequence ofconfigurations. A particular example of a sequence of postures is agraph of postures. Here, a graph of postures denotes a sequence ofpostures which includes a least two postures, which are executed inparallel and mutually exclusive.

The term degrees of freedom, sometimes used in abbreviated form as“dof”, denotes actuated and non-actuated variables that constitute themovement possibilities of a kinematic model.

The configuration space is a vector comprising all degrees of freedom.The configuration space is typically referenced as vector q.

A pose is a spatial description of a rigid body with six degrees offreedom (position and orientation).

The term receding horizon control in robotics refers to a computationalstrategy to re-compute a solution to a task while a robotic system isaddressing the predefined task.

FIG. 1 illustrates a robot-object manipulation scenario in an exemplaryapplication of an embodiment. FIG. 1 depicts a bi-manual robot 2, whichmanipulates a (physical) object 3 using two effectors 8 to perform apredefined task, for example, sorting and building stacks of objects 3in an environment of the robot 2.

FIG. 2 provides an overview of a robotic system 1 and its structuralelements according to an embodiment.

The predefined task achieves a movement of one or several objects 3 inan environment of a robot 2. The at least one object 3 is manipulatedwith one or several end-effectors 9 (manipulators) of the robot 2 orhuman hands. The embodiment addresses in particular the problem offinding contact locations and object poses within the sequence of stepsfor performing the predefined task. The embodiment bases on the conceptof dexterous manipulation, which uses an object-centered description.Okamura, Allison M., Niels Smaby, and Mark R. Cutkosky provide in “Anoverview of dexterous manipulation” in: Proceedings 2000 ICRA.Millennium Conference. IEEE International Conference on Robotics andAutomation. Symposia Proceedings (Cat. No. 00CH37065). Vol. 1. IEEE,2000, a description of this concept, in which multiple effectors 8 ofthe robot 2 cooperate in order to address the task of grasping objects 3and manipulating objects 3.

The method relies on rigid body kinematics and is suited for redundantrobotic systems 1, which in the present context means kinematic models,which have more degrees of freedom than kinematic constraints.

An initial sequence of postures for addressing a task has already beencomputed using a generally known method, e.g. with one of knownstate-of-the-art approaches. For example, a motion planning algorithmdecomposes the task into a sequence of steps. For example, thepublication “Planning support contact-points for acyclic motions andexperiments on HRP-2”, by Adrien Escande, Abderahmane Kheddar, SylvainMiossec, and Sylvain Garsault, in: Experimental Robotics. Springer,Berlin, Heidelberg, 2009, discloses an approach forcontact-before-motion planning for humanoid robots that enablesaddressing the task of finding a path for a mobile robot in aconstrained environment. The initial sequence of postures for addressinga task may be obtained using an A*-search algorithm as discussed in moredetail by Gienger, Michael, et al. in “Human-robot cooperative objectmanipulation with contact changes” 2018 IEEE/RSJ InternationalConference on Intelligent Robots and Systems (IROS), IEEE, 2018.

A set of constraints exists for each step within the sequence ofpostures. The set of constraints may include, e.g., contact locationsand desired motion directions of the at least one object.

A contact refers to a match (spatial correspondence) between one pointand coordinate frame of a body of the robot 2 on the one hand with onepoint and coordinate frame of the environment on the other hand, suchthat a distance between the body and the environment corresponds to(equals) 0. In the field of robotics, the term “coordinate frame” isoften used in an abbreviated form as “frame”.

Preferably, there is a description of invariance for at least one of theconstraints. This means that a constraint in the original pose can bemodified by cancelling one or plural of the elements of the constraintwithout compromising the task. For example, an end-effector 9 of therobot 2 grasps a stick. In this example, there may exist constraintdescription describing the three-dimensional (3D-) description of theend-effector 9 of the robot 2 with respect to the stick. In this examplean invariance in the description is the position along the stick, as theend-effector 9 may grasp the stick at any position along the length ofthe stick. A further description of invariance regards a rotation of theend-effector 9 around the circumference of the stick.

The robotic system 1 of FIG. 2 comprises the robot 2 (bi-manual robot)with two effectors 8, an object tracking device 6, e.g. a camera- ormarker-based object tracking device, and data processing equipment, e.g.at least one computer 4, which is configured to run planning algorithmsand motion generation algorithms. In particular, a program implementingthe method for controlling at least one effector trajectory runs on thecomputer 4.

The computer 4 may include at least one processor, at least one memory,for example comprising non-volatile and volatile memories for storingprogram instructions, and program data generated during execution of themethod.

The robotic system 1 acquires a task description for the predefinedtask, for example via a user interface for receiving instructions to therobotic system 1. For example, the acquired task description (task) mayinstruct the robotic system 1 to put the object 3 on top of a pile offurther objects 3.

The object tracking device 6 determines a current location of the object3 in the environment. The object tracking device 6 generates a sensorsignal 11 and provides the sensor signal 11 to the computer 4.

The computer 4 running a motion planning algorithm decomposes theacquired task into a sequence of steps. This may be done using anA*-search algorithm as discussed in more detail by Gienger, Michael, etal. in “Human-robot cooperative object manipulation with contactchanges”, 2018 IEEE/RSJ International Conference on Intelligent Robotsand Systems (IROS), IEEE, 2018.

The robotic system 1 computes a posture of the robot 2 for each step ofthe sequence of steps and adds all computed postures into an overallkinematic model. The robotic system 1 analyses the sequence of posturesfor contact changes and object motions in each individual step withregard to an immediately preceding step of the sequence of postures. Inparticular, the robotic system 1 applies an algorithm “connect contacts”and an algorithm “connect objects” to the kinematic model. Thesealgorithms will be discussed in detail with reference to FIGS. 6 and 7below.

The computer 4 generates a model representing a task description foraddressing the predefined task. The model includes in particular a setof constraint equations. The generated model is transformed into theinverse kinematics problem. The computer 4 solves the inverse kinematicsproblem applying an optimization algorithm of choice in order to performan optimization process. The optimized constraints, e.g. effector posesand intermediate object poses when re-grasping the object 3, which aredetermined in the optimization process are passed to a trajectorygeneration algorithm running on the computer 4. The trajectorygeneration algorithm may be implemented as discussed in “Task-dependentdistribution and constrained optimization of via-points for smooth robotmotions” by Sung, Changhyun, et al., 2015 IEEE International Conferenceon Robotics and Automation (ICRA). IEEE, 2015.

The generated trajectories for end effectors 9 are passed to an inversekinematics algorithm that computes the corresponding robot postures,which are sent in a control signal 12 to a robot control unit 5. Therobot control unit 5 of the robotic system 1 then controls actuators ofthe effectors 8 of the robot 2 using actuator control signals 10generated based on the control signal 12.

Actuators of the robot 2 may include motors for moving the effectors 8by controlling its joints 8.1, for example.

The robot 2 may generate a status signal 13 and output the status signal13 to the robot control unit 5. The robot control unit 5 may provide theinformation contained in the status signal 13 along with further statusinformation on the robot 2 to the computer 4 in a status signal 14.

The computer 4 may comprise input/output means, for example, outputmeans such as a monitor 7 for displaying image information to a user,and input means such as keyboard and a mouse device for receivingoperation input from the user. The computer may in particular runsoftware implementing a user interface, for example a GUI forinteracting with the user.

FIG. 3 illustrates an object-contact-model for the robot-objectmanipulation scenario in the exemplary application of FIG. 1.

The object 3 is represented with respect to an inertial reference frameby a coordinate chain dof_object. The inertial reference frame may alsobe referenced as world frame. In FIG. 3, contact locations contact1 andcontact2 are represented in coordinates relative to the object 3,illustrated by the coordinate chains dof1 and dof2 in an object frame ofthe object 3. Dots on the lines connecting the object frame with thecontacts contact1 and contact2 (contact points) represent the degrees offreedom that couple the contacts to the object 3. Different mathematicdescriptions for this structure are used, e.g. a combination of atranslation and a quaternion, or three translations followed by threeelementary rotations.

For discussing the embodiment, it is not important how the degrees offreedom are modelled in detail. The description of the spatial contactmovement (contact point movement) with respect to the object 3 isrelated to the known dexterous manipulation concept. The description ofthe contact movement according to the embodiment is not in relation tothe reference frame.

FIG. 3 illustrates a robot model for the robot-object manipulationscenario in the exemplary application of FIG. 1, in particular theobject-centred kinematic model. FIG. 3 illustrates the coupling to theobject-contact model.

The kinematic topology of the robot 2 in FIG. 3 is not part of theobject-contact model of FIG. 3. It is, however, part of the kinematicmodel of a time instance that combines both object 3, the contactscontact1 and contact2, and the robot 2. Kinematic constraints betweenthe effectors 8 of the robot 2 and locations of the contacts ensure thatthe effectors 8 of the robot 2 track the contacts of the robot 2 withthe object 3.

The end-effectors 9 of the effectors 8 of the robot 2 depicted in FIG. 3are hand1 and hand2. The contacts are contact1 and contact2. Thecorresponding kinematic constraints of the scenario in FIG. 3 are(hand1-contact1) and (hand2-contact2).

This modular separation of kinematic models allows to formulate the samepredefined task with robots 2 having different kinematic structures.Thus, the subsequent illustrations of the embodiment, in particularFIGS. 8A to 8D, is therefore able to omit the robot model according toFIG. 3 for a concise description.

FIG. 4 depicts the method for controlling a robot 2 applying anembodiment of the inventive method for motion planning in a simplifiedflowchart.

The method steps implement a process of manipulating the object 3 usingtwo effectors 8 of the robot 2 of the robotic system 1. The displayedrobotic system 1 comprises a bi-manual robot 2 that grasps andmanipulates the physical object 3. The physical object 3 may be a tireor box. Alternatively, the method steps may implement a collaborativerobotic system 1, which performs the predefined task in cooperation witha human also present in the same environment as the robot 2.

In step S1, the method determines an object pose. Step S1 in particulardetermines an initial object pose of the object 3. In step S1, theobject tracking device 6 of the robotic system 1 may determine alocation of the object 3 or locations of objects 3 as initial objectpose within the environment (task environment) of the robotic system 1.

The method may execute step S2 of determining a task objective (taskgoal) in parallel or sequentially to step S1. For example, in step S2,the robotic system 1 may determine the task objective from aninstruction provided externally, for example by a user via the GUIrunning on the computer 4 to the robotic system 1. The robotic system 1may determine the particular task objective from a generic task.

One example of a generic task may include the instruction to sortobjects 3 according to their size and arrange the objects 3 in stacks.The task objective may include grasping a specific object 3 and put thegrasped object 3 in a specific orientation on top of a specific stack ofobjects 3 characterized by including objects 3 of a similar size.

The determined initial object pose and the determined task objectiverepresent a task definition. The task definition is input to the step S3of performing motion planning.

The task definition from steps S1 and S2 provides the basis for the stepS3 of performing motion planning. In step S3, the robotic system 1executes a motion planning algorithm on the task definition in order togenerate a sequence of steps. The steps include a sequence of posturesof the robotic system 1, in particular a sequence of postures of theeffectors 8 of the robotic system 1 and a sequence of object poses toarrive at fulfilling the determined task objective, starting at theinitial object pose. The motion planning algorithm applied in step S3may be one of a plurality of known planning and motion generatingalgorithms available and discussed in literature in order to generatethe sequence of postures provided by the step of motion planning. Therobotic system 1 computes a posture of the robot 2, and in particularthe effectors 8 of the robot 2 for each step of the sequence ofpostures. The computed postures are included in a kinematic model of thetask.

One example for a motion planning system suited to perform step S3 isdiscussed in Gienger, Michael, et al.: “Human-robot cooperative objectmanipulation with contact changes”, 2018 IEEE/RSJ InternationalConference on Intelligent Robots and Systems (IROS). IEEE, 2018.

The robotic system 1 subsequently uses the sequence of postures providedby step S3 of motion planning for performing constraint adaptation instep S4. The step of performing constraint adaptation in step S4includes a sequence of sub-steps, which will be discussed in detail withreference to FIG. 6.

The step S4 in particular includes sub-step S4.2 of applying analgorithm for modifying a contact topology on the kinematic model.Furthermore, step S4 also includes sub-step S4.3 of applying analgorithm for modifying an object topology on the kinematic model.

FIG. 6 discusses the algorithm for modifying a contact topology on thekinematic model according to step S4.2 in detail.

FIG. 7 discusses the algorithm for modifying an object topology on thekinematic model according to step S4.3 in detail.

The contact and object constraints generated in step S4 form the basisfor generating a task level trajectory therefrom in step S5. The contactand object constraints generated in step S4 may, for example, includeeffector poses and intermediate object poses when the effectors 8 are-grasping the object 3.

The adapted contact and object constraints generated in step S4 providethe input to subsequent step S5, in which the adapted contactconstraints and object constraints form the basis for generating thetask level trajectory. Alternatively, step S5 may generate a pluralityof task level trajectories. The step of generating the task leveltrajectory in step S5 includes a sequence of sub-steps, which will bediscussed in detail with reference to FIG. 5.

Generating the task level trajectory in step S5 may be performed usingone of a plurality of known trajectory generating algorithms which willbe discussed with respect to FIG. 5.

The task-level trajectory generated in step S5 is then transformed intothe inverse kinematics problem and the inverse kinematics problem issubsequently solved using an optimization algorithm in step S6. Inparticular, Step S6 computes corresponding robot postures forimplementing the task level trajectory. The computed robot postures mayinclude sequences of joint angles, which the individual joints 8.1 ofthe effectors 8 have to realize for achieving the task objective.

The output of step S6 represents a task description for achieving thetask objective by the robotic system 1. Step S7 uses the generated taskdescription from previous step S6 to control the robot 2 in order toachieve the task objective.

The embodiment depicted in FIG. 4 implements a robotic system 1configured to perform online adaptation. In step S8, an update of theobject pose is performed. The robotic system may perform updating theobject pose based on at least one of the sensor signal 11 and the statussignal 13.

Additionally or alternatively, in step S9, the task objective isupdated. FIG. 4 depicts steps S8 and S9 being sequentially performed,the steps may alternatively be performed at least partiallyconcurrently.

The object tracking device 6 of the robotic system 1 may acquire sensordata for updating the object pose and the task objective. The updatedtask objective and the updated object pose are then used to update thetask definition and to perform motion planning according to step S3using the updated task definition. Thus, the closed loop of theflowchart of FIG. 4 implements an online adaptation system. The onlineadaptation structure with steps S3-S4-S5-S6-S7-S8-S9-S3 according toFIG. 4 is a particularly advantageous structure for performing tasks incollaboration with a human, as the task definition may change due tounpredicted actions of the collaborating human or new instructionsprovided by the collaborating human.

FIG. 5 illustrates steps S4 and S5 of the method for controlling atrajectory according to an embodiment in a flowchart in more detail.

In step S4.1, the robotic system 1 acquires the sequence of posturesgenerated by a motion planning algorithm in step S3 as an initialsequence of postures. Step S4.1 is the first sub-step for performingconstraint adaptation in step S4. The acquired sequence of postures isthen input to step S4.2, in which an algorithm for modifying a contacttopology on the kinematic model is applied in order to generate amodified contact topology of the kinematic model. FIG. 6 illustrates thealgorithm for modifying a contact topology on the kinematic modelaccording to step S4.2 in detail.

The method proceeds to step S4.3 of applying an algorithm for modifyingan object topology of the kinematic model to generate a modified objecttopology of the kinematic model. FIG. 7 illustrates the algorithm formodifying an object topology on the kinematic model according to stepS4.3 in detail.

The modified kinematic model with the modified contact topologyaccording to step S4.2 and the modified object topology according tostep S4.3 are subsequently used for generating a set of constraintequations in step S4.4 based on the modified kinematic model.

The step S4.4 of generating the constraint equations may be performedaccording to generally known processes discussed in the literature inthe field of robotics. In the following, two exemplary and alternativeapproaches for implementing step S4.4 are summarily discussed: a firstapproach corresponds to a task level constraint formulation. A secondapproach is a kinematic joint coupling formulation.

The first approach based on the task-level constraint formulation usestask-level constraints, which are included in a constraint vector δx andhave a corresponding constraint Jacobian matrix J_(c). Initially, akinematic model is generated that comprises all steps of the sequence ofpostures. For a sequence of postures comprising n individual postures(steps), the corresponding kinematic model comprises

n×dof_i;  (1)

degrees of freedom. In expression (1), dof_i denotes the number ofdegrees of freedom of an individual step of the sequence of steps.Furthermore, a constraint vector includes all kinematic constraintequations of the entire sequence of postures. The kinematic constraintequations describe the contacts and the object movements in the sequenceof postures. Thus, the kinematic constraint equations include theinformation generated by performing the processes “connect contacts” instep S4.2 of modifying a contact constraint topology and “connectobjects” in step S4.1 of modifying the object topology in the precedingsteps of the method. Examples for the algorithms are explained as thealgorithm “connect contacts” depicted in the flowchart of FIG. 6 and asthe algorithm “connect objects” in the flowchart according to FIG. 7.

Mathematically, a formulation of the problem according to the task-levelconstraint formulation may read:

$\begin{matrix}{{{\begin{pmatrix}J_{T} \\J_{c}\end{pmatrix}\delta q} = \begin{pmatrix}{\delta x_{T}} \\{\delta x_{c}}\end{pmatrix}};} & (2)\end{matrix}$

In equation (2), the term J_(T) comprises all task objectives that arenot part of the contact constraints and object motion constraints, e.g.a desired movement of the object or a desired movement of effectors 8,or a movement of a motor-driven camera. Matrix J_(c) is the constraintJacobian matrix that is assembled with the sequence of postures and theobject movement constraints. The vector δq comprises a displacement ofthe degrees of freedom q of the entire kinematic model including allsteps of the sequence of postures. The vector δx_(T) comprises thedesired displacements for all task objectives that are not part of thecontact constraints and object motion constraints. The term δx_(c)comprises all constraint displacements. The constraint displacementsδx_(c) are typically zero.

Thus, a kinematic model including all steps of the sequence of posturesis generated. The kinematic model comprises all constraint equationswithin one holistic linear equation system according to equation (2).

This is a conventional way to formulate such constraints. The number ofall task-level constraints corresponds to the dimension n of the vectorδx=(δx_(T) ^(T) δx_(c) ^(T))^(T). This requires solving the linearequation system (2), which has an order O (n³) with n denoting thenumber of constraint equations. It is noted, that, in terms ofcomputation time, this formulation scales sufficiently well for a smallnumber of constraints.

The second approach of the kinematic joint coupling formulation providesan approach whose modelling proves advantageous in terms of scalabilityof the computational effort required for a large number of constraints.The basic concept of kinematic joint coupling is to directly couple theconstraints on the level of the degrees of freedom. It uses constraintprojection techniques that are also applied in multi-body dynamicscomputations. Essentially, a set of degrees of freedom that is coupledkinematically is subsumed in one virtual degree of freedom. Formally,this can be expressed as

J_(c)*=J A:  (3)

Equation (3) describes a projection of degrees of freedom on a virtualdegree of freedom. Matrix A is a projection matrix, which projects thecoupled degrees of freedom q onto the virtual degrees of freedom q*.Thus, equation (2) may be rewritten using equation (3) as

J _(c) *δq*=δx;  (4)

Equation (4) describes a virtual-degree of freedom based constraintformulation. This constraint formulation has the advantage that theexplicit constraint displacements δx_(c) of equation (2) are notincluded. A dimension of the virtual degrees of freedom δq* is less thana dimension of the degrees of freedom δq. The reduction in dimensionimproves the scalability of solving the resulting linear equation systemsignificantly.

Applying kinematic joint coupling formulation according to the secondapproach on the exemplary sequence of postures illustrated in FIG. 8Ashows that the problem can be solved five times faster than when usingthe first approach of task-level constraint formulation.

Returning to the flowchart of FIG. 5, the step S5 of generating atrajectory is discussed in more detail using FIG. 5, in particular itslower portion.

The generated set of constraint equations from step S4 and sub-step S4.4is subsequently used for sub-steps S5.1, S5.2 and S5.3 according to theembodiment of FIG. 5. Step S5, generates a trajectory, in particular aneffector trajectory or effector trajectories. In step S5.1, a constraintrelaxation is performed on the generated set of constraint equationsfrom step S4, resp. sub-step S4.4. Step S5.1 may perform the constraintrelaxation based on a relaxation strategy on the constrained contactsand the constrained objects, which model the interdependencies ofmotions of the object 3 and the contacts contact1, contact2 between theindividual steps of the sequence of postures. The constraint relaxationof step S5.1 enables to efficiently utilise the concepts of the processof constraining the contact topology according to step S4.2 andconstraining the object topology according to step S4.3.

The relaxation strategy is a strategy or a set of strategies, whichchanges mathematically strict constraints to relaxed constraints.Mathematically strict constraints are constraints, which ensure aprecise constraint satisfaction. The relaxed constraints areconstraints, which in comparison to the mathematically strictconstraints before step S5.1 achieve the result that a Null space motionwill be projected into relaxed constraint coordinates. Performingconstraint relaxation may include in particular

(1) allowing a deviation of the constraint instead of enforcing theoriginally strict constraint.

An example for the approach according to (1) for performing constraintrelaxation is regularization of individual constraints within theinverse kinematics, such as the Damped Least Squares approach accordingto: “Review of the damped least-squares inverse kinematics withexperiments on an industrial robot manipulator” by Chiaverini, Stefano,Bruno Siciliano, and Olav Egeland. In: IEEE Transactions on controlsystems technology 2.2 (1994): 123-134. A further approach for relaxingconstraints in step S5.1 includes

(2) removing constraint coordinates that are invariant for the task.

European patent EP 1 728 600 B1 discloses the approach according to (2)for controlling an effector trajectory from a current state to a targetstate whereby invariant control parameters are taken into account.Invariant control parameters are control parameters that are notrequired, but do not interfere to achieve the predefined task. This maycorrespond to an end effector rotation about a cylindrical object 3. Theeffector trajectory is then represented in a task description, the taskdescription being void of the invariant control parameters. As the taskdescription does not include the invariant control parameters, adimension of the Null space is increased. Yet a further approach forrelaxing constraints in step S5.1 includes

(3) removing constraint coordinates that are within a pre-definedinterval or region.

Particular examples for the approach according to (3) for relaxingconstraints provide, for example, Gienger, Michael, Herbert Janßen, andChristian Goerick in “Exploiting task intervals for whole body robotcontrol”, 2006 IEEE/RSJ International Conference on Intelligent Robotsand Systems. IEEE, 2006; Sugiura, Hisashi, et al. in “Real-time selfcollision avoidance for humanoids by means of nullspace criteria andtask intervals”, 2006, 6th IEEE-RAS International Conference on HumanoidRobots. IEEE, 2006; Berenson, Dmitry, et al. in: “Pose-constrainedwhole-body planning using task space region chains”, 2009 9th IEEE-RASInternational Conference on Humanoid Robots. IEEE, 2009, and Gienger,Michael, Marc Toussaint, and Christian Goerick. “Task maps in humanoidrobot manipulation.” 2008 IEEE/RSJ International Conference onIntelligent Robots and Systems. IEEE, 2008.

These exemplary relaxation strategies for performing constraintrelaxation in step S5.1 achieve the effect that the Null space motion isprojected into the relaxed constraint coordinates. Illustrative examplesinclude

-   -   a rotation of an effector-contacting surface about a contact        normal vector,    -   a displacement of contacts along an edge or a face of a        geometric structure,    -   an acceptable inclination of an effector contact with respect to        the contacting surface, and    -   an object 3 to be re-grasped at an arbitrary position or with an        arbitrary pose.

Performing the constraint relaxation of step S5.1 leads to consistentlyadjusting the constraint coordinates so that a cost function H (q) willbecome optimal. This effect will be discussed with reference to anexample shown in FIG. 8 below.

Step S5.1 generates a relaxed set of constraint equations by performingconstraint relaxation. Step S5.2, succeeding to step S5.1, performs anoptimization on the relaxed set of constraint equations based on a costfunction

In step S5.3, the optimization result of the relaxed set of constraintequations provides the basis to generate an effector trajectorytherefrom. In particular, the relaxed set of constraint equationsprovides the basis to generate or to optimize the effector trajectory.The possibly optimized task level trajectory is provided to step S6 forperforming inverse kinematics with the task level trajectory as input togenerate the control signal 11, and subsequently to controlling therobot 2 based on the corresponding control signal 11.

Solving the mathematical formulation of the posed problem modelledeither by the task-level constraint formulation or the kinematic jointcoupling may be performed in a corresponding manner. A motion of thedegrees of freedom δq or δq* is computed in step S5.3 using inversekinematics performing a generally known approach. For example,publication “Automatic supervisory control of the configuration andbehavior of multibody mechanisms” by Liegeois, Alain, in: IEEEtransactions on systems, man, and cybernetics 7.12 (1977): 868-871discusses a suitable approach in detail.

Alternatively, any other differential inverse kinematics algorithm withredundancy resolution may be used.

An aspect of the inverse kinematics calculation is the resulting Nullspace motion. A Null space motion typically exists when a model has moredegrees of freedom than constraints. Such kinematic models are redundantmodels. In the present context, it is assumed that the generated modelis a redundant model. Redundancy resolution is a concept to project ascalar cost term H (q) into the Null space of the motion.

The Null space motion is the motion of the system that does not changeany of the task-level constraints in δx.

The cost term H (q) is described in the configuration space q of therobot 2. The configuration space is the space of the controllabledegrees of freedom of the robot 2. The configuration space may becomposed of individual joints 8.1 of a robot 2 or more complex kinematicmechanisms to which controllable degrees of freedom can be assigned.

Typical examples for the cost term H (q) describe avoiding joint limits,or avoiding self-collisions with regard to the effectors 8 of the robot2. The particular choice of the cost function H (q) as criterion is notimportant in present context. Computing the motion of the overallsequence of postures under consideration of all degrees of freedom q,all task-objectives and all constraints δx and the cost function H (q)may be formulated as

δq=J _(c) *δx−N grad(H (q))^(T); (5)

Equation (5) describes redundant differential inverse kinematics. MatrixJ_(c)# refers to the pseudo-inverse of the Jacobian matrix J_(c). MatrixN is the corresponding Null space projection matrix. The gradientoperator grad (.) in equation (5) defines the gradient of the cost termH (q) with respect to q.

The cost term H projection has an important role in present context, andhad been discussed in the section with regard to step S5.1 of performingrelaxation on the generated set of restraint equations, since it definesthe gradient direction of the objective of H (q).

Equation (5) may be solved efficiently using gradient-based optimizationtechniques. For example, known line-search based algorithms perform wellfor solving problems such as equation (5).

FIG. 6 depicts a flowchart for the process of connecting contactconstraints according to step S4.2 across the sequence of postures in animplementation of the invention.

FIG. 7 depicts a flowchart for a process of connecting objects acrossthe sequence of postures in an implementation of the invention. Theconcept of the algorithm “connect object” underlying method step S4.3.

FIGS. 6 and 7 will be discussed in detail after presenting their genericconcept using FIGS. 8A to 8D.

FIG. 8A illustrates a robot-object manipulation scenario including asequence of steps in an exemplary application of an embodiment.

The depicted scenario considers a sequential task such as turning theobject 3 upside-down. The scenario can be represented as a sequence ofthe above kinematic models. FIG. 8A shows seven individual posturesranging from step 1 to step 7 of the depicted sequence of postures wherespatial changes of contacts and movements of the object 3 occur.

FIG. 8B shows a sequence model of the manipulation scenario according toFIG. 8A comprising the sequence of postures without coupling ofconsecutive steps.

FIG. 8B displays the corresponding kinematic model for the sequence ofpostures according to FIG. 8A. The kinematic chain of each contact isfrom “contact” via “object” to “reference frame”. Therefore, each stepfrom step 1 to step 7 has a corresponding posture that is independent ofthe posture of all other steps of the depicted sequence of postures.

A first process of the method is to modify a contact constraint topologyaccording to the sequence of postures as shown in FIG. 8B. The result ofperforming the method to modify the contact constraint topology isdepicted in FIG. 8C.

In FIG. 8B, from step 1 to step 2, the right contact does not change itslocation on the object 3, it remains fixed in the object coordinateframe. According to the process of modifying a contact constrainttopology, the kinematic constraint of the right contact is amended toconnect to the respective contact of the previous step. In FIG. 8C, anarrow connecting the right contact of step 2 with the correspondingright contact of step 1 is added. The kinematic chain of the rightcontact in step S2 is cancelled.

From step 2 to step 3, both contacts remain fixed in the object'scoordinate frame. The object 3 is rotated from step 2 to step 3 in FIG.8B. Therefore, both contacts of step 3 are connected to thecorresponding contacts of step 2. The kinematic chains of the rightcontact and the left contact in step S3 are cancelled.

This strategy of modifying a contact constraints topology according tothe sequence of postures as shown in FIG. 8B is performed until the endof the sequence, e.g. step S7 of the sequence of postures is reached.FIG. 8C shows the resulting sequence model with a modified contactconstraint topology.

FIG. 8C shows the sequence model of the manipulation scenario accordingto FIG. 8A comprising a sequence of postures with a coupling of fixedcontacts of respective consecutive steps of the sequence of postures.

While the embodiment discussed with reference to FIG. 8A to 8D, uses abi-manual robot model for illustration, it is evident that the conceptis applicable to any number of contacts, and also to scenarios involvingmore than one object 3.

Furthermore, the concept is also not limited to the effectors 8 of arobot 2 (robot arms). Anything that can be mathematically represented asa chain of rigid bodies (tree of rigid bodies) may be understood to becovered by the term “effector”.

Furthermore, the concept is also not limited to effectors 8 of astationary robot 2. The concept is also applicable for, e.g. mobileplatforms, multi-legged robots 2 or bio-mechanical models of a human. Inone particular application, the effectors 8 of a (mobile) robot 2 maycorrespond to means for moving the robot 2, such as legs (robot legs).

FIG. 8D shows a sequence model of the manipulation scenario according toFIG. 8A comprising a sequence of postures with a coupling of fixedcontacts of the consecutive steps and a coupling of constant objectposes between consecutive steps. FIG. 8D illustrates the sequence modelof the manipulation scenario according to FIG. 8A based on modifiedconstraint and object topology in the generated set of constraintequations. With regard to the sequence of postures with a coupling offixed contacts of the consecutive steps reference to FIG. 8C and thecorresponding section of the description is considered sufficient.

A second process of the method is to modify an object constrainttopology according to the sequence of postures. This process may beperformed independent of the first process.

Alternatively, the second process of the method of modifying an objectconstraint topology according to the sequence of postures may beperformed in combination with the first new process. In FIG. 8D, theresult of performing the first and the second new process is depicted.

FIGS. 8A to 8D, which base on the same example sequence of postures,show several transitions between steps S1 to S7, in which the effectors8 change their spatial location, but the pose of the object 3 betweenconsecutive steps remains constant in the inertial reference frame. Thisis the case, for example, for the transition from step 1 to step 2 aswell as for the transition from step 3 to step 4, from step 4 to step 5,and from step 6 to step 7.

If an object pose in the reference frame of an individual step does notchange with respect to the immediately preceding step, a kinematicconstraint of the object pose to the step's reference frame is deleted,and a new constraint to the object pose in the preceding step isgenerated and added. In the depicted example of FIGS. 8A to 8D, newobject constraints are added from step 1 to step 2, from step 3 to step4, from step 4 to step 5, and from step 6 to step 7. Simultaneously,kinematic constraints of the object pose to the step's reference frameare deleted in steps 2, step 4, step 5 and step 7, as shown in FIG. 8D.

FIG. 6 depicts a flowchart for a process of connecting contactconstraints across the sequence of postures in an implementation of theinvention. The process is an embodiment of method step S4.2 formodifying the contact topology of the sequence of postures S. Theprocess of connecting contact constraints is performed on the sequenceof postures S including postures (steps) i, i ranging from i=1, 2, . . ., n and contacts j, j ranging from j=1, 2, . . . , m.

In step S4.2.1, a step identifier is set to i=2. The method proceeds tostep S4.2.2, in which a contact identifier is set to j=1.

In step S4.2.3 succeeding to step S4.2.2, the process checks whether acontact location of contact j in object coordinate frame in step i didchange with respect to contact j in step i−1.

If the answer in step S4.2.3 is YES, meaning that the contact locationof contact j in object coordinate frame in step i did indeed change withrespect to contact j in step i−1, the method proceeds to step S4.2.5. Instep S4.2.4, the constraint to object in step i is removed. In stepS4.2.5 succeeding to step S4.2.4, the constraint of contact j in step iis connected to contact j in step i−1. The method then proceeds to stepS4.2.6.

If the answer in step S4.2.3 is NO, meaning that the contact location ofcontact j in object coordinate frame in step i did not change withrespect to contact j in step i−1, the method proceeds directly to stepS4.2.6.

In step S4.2.6, the contact identifier is increased by one from j toj+1. From step S4.2.6, the method proceeds to step S4.2.7. In stepS4.2.7, the method determines whether all contacts of a current step ihave been asserted for changes with respect to their correspondingcontact location the in object coordinate frame to the contact locationin the immediately preceding step i−1. In particular, step S4.2.7determines whether the contact identifier j is equal to m.

If the answer in step S4.2.7 is NO, the method assumes there remaincontacts of the current step i not been tested yet. In this case, themethod returns to step S4.2.3 and repeats the steps S4.2.3, S4.2.4,S4.2.5, and S4.2.6 (inner loop) for the next contact having theincreased contact identifier j+1.

If the answer in step S4.2.7 is affirmative (YES), the method assumesall contacts of the current step i having been tested and the methodsucceeds to step S4.2.8 and increments the step identifier i by one toi+1. The method then proceeds to step S4.2.9. In step S4.2.9, the methoddetermines whether the all steps i of the sequence of postures S havebeen tested with regard to change of their spatial contact location withrespect to the preceding step. In particular, step S4.2.9 determines,whether step identifier i equals n+1.

If the step identifier does not equal 1, e.g. the answer in step S4.2.9is NO, the method returns step S4.2.2 and sets the contact identifier toj=1. The method proceeds by checking all contacts j for the new step i.

If the step identifier does indeed equal 1, e.g. the answer in stepS4.2.9 is YES, the method assumes that all steps i between step 2 andstep n of the sequence of postures have been checked for change incontact locations of all contacts with regard to the respectivepreceding step and terminates the process of connecting contacts andstep S4.2 of modifying the contact topology of the method.

The concept of the algorithm “connect contacts” underlying method stepS4.2 to connect the constraints across the sequence can be summarized:

Algorithm “connect contacts” For each step s_i of sequence S: Didcontact location in object coordinate frame change with respect to theprevious step s_i−1?   No: Remove constraint to object in step s_i andconnect   constraint to contact point of previous step;   Yes: Leavecontact constraint as it is; End foreach.The discussed example of the algorithm “connect contacts” regards asequence of postures with an exclusively sequential structure.Alternatively or additionally, a particular example of the sequence ofpostures may have a structure including closed chain of postures. In asequence of postures representing a closed chain of postures,constraints of a first posture in the sequence of postures are connectedto a last posture of the sequence of postures. FIG. 7 depicts aflowchart for a process of connecting objects across the sequence ofpostures in an implementation of the invention. The concept of thealgorithm “connect object” underlying method step S4.3 to connect theconstraints across the sequence of postures can be summarized as:

Algorithm Connect Objects: For each step s_i of sequence S:  Did objectpose change with respect to the previous step s_i−1?  No: Remove objectconstraint with respect to reference frame  and connect object of steps_i with constraint to previous step;  Yes: Leave object constraint asit is; End foreach.The discussed example of the algorithm “connect objects” regards asequence of postures with an exclusively sequential structure.Alternatively or additionally, a particular example of the sequence ofpostures may have a structure including a closed chain of postures. In asequence of postures representing a closed chain of postures,constraints of a first posture in the sequence of postures are connectedto a last posture of the sequence of postures.

In particular, the process is an embodiment of method step S4.3 formodifying the object topology of the sequence of postures. The processof connecting contact constraints is performed on the sequence ofpostures S including postures (steps) i, wherein i is an integer betweeni=1, 2, . . . , n.

In step S4.3.1, a step identifier is initialized to i=2. The methodproceeds to step S4.3.2, in which the method determines whether anobject pose of the object in current step i changes with respect to theobject pose of the object in the immediately preceding step i−1 of thesequence of postures.

If the answer in step S4.3.2 is NO, meaning that the contact location ofcontact j in object coordinate frame in step i did not change withrespect to contact j in step i−1, the method proceeds to step S4.3.3. Instep S4.3.3, the constraint of the object to the reference frame isremoved in the current step i. The method then proceeds to step S4.3.4.In step S4.3.4, the constraint of the object in step i is connected tothe object in step i−1, the immediately preceding step to the currentstep i in the sequence of postures. The method then proceeds to stepS4.2.5.

If the answer in step S4.3.2 is YES, meaning that the object pose of theobject coordinate frame in step i did indeed change with respect to theobject pose of the object in the immediately preceding step i−1 of thesequence of postures, the method directly proceeds to step S4.2.5,skipping the steps 4.3.3 and S4.3.4.

In step S4.2.5, the step identifier i is incremented by one from i toi+1. The method then proceeds to step S4.3.6. In step S4.3.6, the methoddetermines whether the all steps i of the sequence of postures have beentested with regard to change of their spatial object pose with respectto the immediately preceding step of the sequence of postures. Inparticular, step S4.3.6 determines, whether step identifier i equalsn+1.

If the step identifier does not equal 1, e.g. the answer in step S4.3.6is NO, the method proceeds with step S4.3.2. The method proceeds bychecking the object pose for new step i+1.

If the step identifier does indeed equal 1, e.g. the answer in stepS4.3.6 is YES, the method assumes that all steps i between step 2 andstep n of the sequence of postures have been checked for a change inobject pose with regard to the object pose in the immediately precedingstep and terminates the process of connecting objects, and step S4.3 ofmodifying the object topology of the method.

FIG. 9 illustrates a graph representation of the sequence of posturesaddressing the task comprising two alternative branches in animplementation of the invention.

In particular, FIG. 9 depicts a graph representation of the sequence ofpostures with a decision point between step 2 and step 3. Extendingbeyond the exemplary sequence of postures shown in FIGS. 8A to 8D, whichillustrate a coupling between steps in a linear sequence of posturesfrom step 1 to step 7, the present invention is not limited to thislinear sequence of postures. The same basic concept can also be appliedto tasks that have a graph structure.

A task having a graph structure may, for example, realize addressing thetask objective using two or more different options.

FIG. 9 illustrates the graph structure for a directed acyclic graph. Asdepicted in FIG. 9, after step 2, the task can be realized by eitherproceeding with rotating the object clock-wise in a first option, or byrotating the object counter-clockwise in an alternative second option.

The first option is displayed in the upper branch of FIG. 9. The secondoption is displayed in the lower branch of FIG. 9.

After step 6, both the first and the second option merge to the finalstep 7 again, which is the same step 7 for both options. Modellingseveral options permits a variety of application cases for embodimentsof the invention.

For example, the pose in step 2 of the sequence of postures can becomputed such that both the first option and the second option can bepursued as equally optimal.

Alternatively, the first and second options can each be weightedaccording to their quality.

Yet alternatively, the first and the second option may simply beselected.

FIG. 9 shows an example of a sequence of postures, which branches at thedecision point of step S2 into two options. It is evident, that theconcept may be extended to more than two options and to sequences ofpostures with more than one decision point.

In an embodiment, a user may perform the weighting of options or theselecting of options interactively via a user interface provided by acomputer program.

FIG. 10 illustrates a sequence of postures before and after anoptimization of the sequence of postures. The task addressed in FIG. 10includes rotating of a box-shaped object 3 by the robot 2 with twoeffectors 8 around a horizontal axis. The step of constraint relaxationresults in consistently adjusting the constraint coordinates so that thecost function H (q) will become optimal. This is illustrated in FIG. 8,which refers to the first three steps of the sequence of posturesaccording to FIGS. 8A to 84D.

FIG. 10 shows the three steps, step 1, step 2, and step 3, of anexemplary sequence of postures. The upper portion depicts the robotpostures before an optimization is performed. The upper portioncorresponds to a value of the cost function H (q) that is 0.617.

The left end effector of step 2 and 3 are coupled. A rotation of theend-effector 9 around the normal vector of the box face is allowed. Whenperforming constraint relaxation in step S5.1, the correspondingconstraint is therefore removed.

The lower portion of FIG. 10 illustrates the resulting postures afterperforming a constraint relaxation according to step S5.1 and subsequentoptimization of step S5.2. A resulting cost value of the cost function H(q) that is 0.438, which is an improvement of about 30%, when thereducing the cost value numerically corresponds to improving thesolution. FIG. 8 shows the resulting angle between end-effector 9 of therobot 2 and box 3 is depicted with black lines. FIG. 8 shows that anglesin step 2 and 3 are the same. For the optimized solution, the angledecreased in the lower portion of FIG. 8, but the angles in the lowerportion of FIG. 8 are still consistent between step 2 and step 3 of thesequence of postures.

Embodiments of the method may be applied in areas of robotics, forinstance sequential assembly tasks and shared autonomy tasks with humanguidance. More specifically, the method enables real-time robot motionplanning for handling, manipulating and transporting objects. The methodprovides planning contacts for robot location and end-effectorplacement, as well as planning a sequence of object poses to achieve thegiven task objective. The invention also allows to find optimalsolutions how a robot 2 grips a tool as specific example for a(physical) object 3.

The method enables to determine optimal motions for robot-robotcooperative tasks, such as hand-over of tools or work pieces. Thereforethe method may be advantageously applied in the field of multi-robotplanning.

The method is not limited to the field of robotics, but enables toperform realistic computer animation and simulation of kinematicmovement of virtual characters and structural objects. The virtualcharacters may be animated how to perform a sequence of objectmanipulation steps. Creating training videos, in augmentedreality/virtual reality (AR/VR-) environments, in interactive workspacedesign software, and in computer games is supported by the method.

The virtual characters may be employed as training examples how a humanadvantageously performs the task. For example, in sport and homecomputer games, the method supports players in identifying a sequence ofpostures (moves), for instance which handholds to use in a wall-climbinggame, or how to set feet and hands in the twister game. In a factorysetting, the sequence postures describing how to grasp and re-grasp anobject 3 such as a heavy car door to be mounted onto a hinge arranged ata chassis hinge can be determined and shown to a trainee worker usingAR/VR technologies. Similarly, an optimized support surface foraddressing tasks that require leaning over large items while performingthe task can be visualized to the trainee worker.

Changing an underlying criteria function, e.g. a load functiondescribing a load on a human spine supports understanding which postureshould be avoided, and which posture is suited when addressing the task.This can be utilized in training animations to show the differencebetween suitable and less suitable ways to perform the task.

The effect of applying the method for arranging elements and objects 3in a work cell, and their immediate effect on the way a worker needs tomove for addressing the task, allow designers to better learn andunderstand the relation of their designs to the worker's ergonomics.This can be animated directly in a computer program, or in VR/ARinterface. Alternatively or additionally, computer-aided design toolsmay be designed that include an embodiment. The computer-aided designtool may be equipped with a slider as an input interface enabling tomodify at least one parameter of a criteria function, e.g. a humanergonomic criteria function. The slider is an advantageous toolillustrating how the design of the work environment changes with respectto the chosen criteria function. Thus, the method enables human-centeredmotion planning in an advantageous manner. Given an ergonomic model of ahuman collaboration partner, the method adjusts end-effector posturesand object poses in order to match a quality function that comprises anergonomic state of the human. The robot 2 may, for instance, adjust theobject pose for an ergonomically optimal hand-over to a human based onthe results of the method.

In the field of human-centered motion planning, the concept of themethod enables incorporating a model of a human into the algorithm. Themodel of the human can, for example include a bio-mechanic description,a kinematic model or an ergonomic state of the human. Starting from thegiven model, the method proceeds by modelling an interaction with thehuman using the sequential constraint concept of the method. Applyingthe method enables adjusting hand- and object poses of the robot 2 inorder to match a quality function that comprises human-centeredquantities. Particular examples include

-   -   adjusting the object pose in a human-robot hand-over task for an        ergonomically optimized hand-over to the human,    -   determining how to hold the hand of a person that is to be        dressed assisted by the robot 2, while pulling over a sleeve        over an arm of the person, in the application area of        robot-assisted dressing,    -   tasks including human-robot collaborative manipulation of large        objects 3, e.g. boxes, the concept enables to find a sequence of        object poses that reduce loads on the spine of the human,        leading to reduced lower back injuries, and    -   optimizing physical support systems that aid elderly or        handicapped persons to get up from a lying or sitting pose, the        method may be applied to determine a spatial effector location        for offering a supporting hand hold, and how to move the        effector 8 so that the patient is supported optimally with        respect to his or her physical abilities and ergonomic        condition.

The method may be used for optimizing a location of the robot 2 based onaccompanying simulations of a given task. The method may determine theoptimized location from a description of the task, by simulating thesteps to address the task, and performing relaxation in particular onthe constraint on the location of the robot 2. Thus, the method allowsto find an optimized location of the robot 2 with respect to thepredefined task.

The method may be employed to simulate the given task using differenttypes of robots 2 in order to determine, which type of robot 2 is mostsuited for performing the given task.

A further advantageous application area for the method is in work-placedesign based on one or preferably plural simulations applying themethod. For example, the method is used in a computer program adapted todesigning the work-place within a factory environment. Starting from agiven description of the predefined task, the method may simulate thesteps to address the task. In particular, the method performs relaxationof the constraint on the location of the tools and physical objects 3related to the task according to the description. Applying the methodresults in determining optimized locations of the required tools andobjects 3 required for the performing the predefined task.

The method may be advantageously applied for finding optimal motions forperturbations in a task. The method determines optimized solutions forproblems, in which exact coordinates of end-effectors 9 and objects 3involved in the task are unknown, or the coordinates vary to certaindegree. The method addresses this particular problem by addingperturbations of the task as additional sequences of postures S to theoverall model of the task, and to optimize the overall perturbations asone ensemble. A particular example is the “grip-in-the-box”-problem, inwhich a robot 2 is required to grasp objects 3 from different locationsout of a box.

The method enables a predictive decision making in a particularadvantageous manner. A sequence of postures is extended to a graph ofpostures or a tree-like structure. Each branch of a tree represents analternate option for proceeding from a decision point. The treeseparates at the decision point into separate branches, wherein eachbranch corresponds to one of a number of alternate options. Whenmanipulating an object, an example of the decision point may include twoalternatives in proceeding, either in a first option “turning the objectleft” or in a second option “turning the object right” when proceedingwith the sequence of postures. The method allows without any extensionmodelling such tree topologies, which makes the method unique andadvantageous with respect to known trajectory optimization algorithms.Potential applications include applying the method in online recedinghorizon motion planning. In online receding horizon motion planning as aparticular example, a motion can be computed such that it is optimizedwith respect to the first option or the second option, or a weightedaverage of all possible options at a same time. Thus, decision makingbetween a first option and a second option may be based on computedcriteria.

The embodiment of the invention focuses on examples characterized by asequence of postures with linear structure. However, extending theconcept of a linear sequence to a tree structure including two or morebranches provides further advantageous embodiments. The cost functioncorresponding to each branch may be weighted so that a contribution ofthe cost function of each individual branch to the overall cost functioncan be modulated. Weighting both options in a tree structure with twobranches equally will result in a sequence of postures that is optimizedwith respect to both options. This results in determining motions thatallow to take a decision for one of the two branches, therefore for oneout of the two options, as late as possible.

Provided that some measure describing which one of the alternate optionsis more likely is known, the options may be weighted according to theirrespective likelihood. If, for instance, in known cases, a left effectorof the robot has been preferred over a right effector of the robot forgrasping an object with a ratio of 80% to 20%, the known ratio may beused to modulate both options accordingly.

Using the same model, the sequence of postures may be computed using thefirst option only, and then second option only. Subsequently, thecomputed results may be compared, and a decision for one of the firstand the second option can be taken based on a comparison of the costfunctions for both options.

In case there exists a large number of possible alternate options, aweighting of the alternate options can be included into an optimizationproblem, providing solutions that are void of the options with the leastquality, and maintain the options which are most promising.

The method may be applied in in-hand manipulation of objects 3. Given aninitial contact sequence in the initial sequence of postures, the methodenables determining optimized contact sequences for an in-hand objectmanipulation task.

The method is applicable to tasks such as object transportation orobject manipulation with an unknown number of re-grasps by theend-effectors 9. In case the number of re-grasps or contact changes isunknown, the method may be applied to compute several sequences ofpostures, each sequence with a different number of postures (steps), andto determine the minimum number of postures by rejecting all computedsequences that deliver motions leading to a violation of at least onephysical limit of the robot 2.

In the area of multi-contact locomotion of a mobile robot 2, the methodmay be applied to optimize multi-limb locomotion patterns for a humanoidor multi-legged robot 2.

The method of present invention enables to control the robot 2, inparticular at least one effector trajectory of at least one effector 8of the robot 2. Controlling the effector trajectory has effects(physical effects) in the real world, for example in changes of spatialcoordinates of the at least one effector 8 and therefore movement of theat least one effector 8 in the real world, which can be expressed inphysical parameters. The robustness of robot control may be improved, apower consumption of the robot 2 may be reduced and the robot 2 operatesmore efficient. The computational effort for computing the at least oneeffector trajectory is advantageously reduced when applying the method.The method is advantageously fast and therefore well suited for onlineand real-time application, and thus in a robotic system 1 controlledaccording to the principle of receding horizon control.

What is claimed is:
 1. A method for controlling at least one effectortrajectory of an effector of a robot for solving a predefined task, themethod comprising: acquiring a sequence of postures, each postureincluding at least one contact point with a physical structure and akinematic pose of the at least one effector; modifying at least one of acontact constraint topology according to the acquired sequence ofpostures, and an object constraint topology according to the acquiredsequence of postures; generating a set of constraint equations based onat least one of the modified contact constraint topology and themodified object constraint topology; performing constraint relaxation togenerate a task description including a set of relaxed constraintequations; generating the at least one effector trajectory by applying atrajectory generation algorithm on the set of constraint equations;performing an inverse kinematics algorithm on the generated at least oneeffector trajectory for generating a control signal; and controlling atleast one effector to execute the at least one effector trajectory basedon the generated control signal.
 2. The method for controlling at leastone effector trajectory according to claim 1, wherein modifying thecontact constraint topology comprises: in case a contact location in theobject frame of a current posture in the sequence of postures remainsthe same as a contact location of the corresponding contact in animmediately preceding posture of the sequence of postures; removing aconstraint of the contact to the object in the current posture; andconnecting a new constraint of the contact in the current posture to thecorresponding contact in at least one immediately preceding posture ofthe sequence of postures.
 3. The method for controlling at least oneeffector trajectory according to claim 1, wherein modifying the objectconstraint topology comprises: in case an object pose of the currentposture in the sequence of postures remains the same as the object posein an immediately preceding posture of the sequence of postures;removing a constraint of the object in the current posture; andconnecting a new constraint of the object in the current posture to thecorresponding object in at least one immediately preceding posture ofthe sequence of postures.
 4. The method for controlling at least oneeffector trajectory according to claim 1, wherein the sequence ofpostures is a linear sequence.
 5. The method for controlling at leastone effector trajectory according to claim 1, wherein the sequence ofpostures has a graph structure modelling at least two alternate optionsfor performing a task by controlling the at least one effectortrajectory.
 6. The method for controlling at least one effectortrajectory according to claim 1, wherein performing constraintrelaxation on the generated set of constraint equations comprises:regularizing at least one constraint by allowing a deviation from theindividual constraint.
 7. The method for controlling at least oneeffector trajectory according to claim 1, wherein performing constraintrelaxation on the generated set of constraint equations comprises:removing at least one constraint which is invariant for performing thetask.
 8. The method for controlling at least one effector trajectoryaccording to claim 1, wherein performing constraint relaxation on thegenerated set of constraint equations comprises: removing at least oneconstraint in case a value of the constraint coordinate is within apredetermined interval.
 9. The method for controlling at least oneeffector trajectory according to claim 1, wherein each contact point ismodelled in a kinematic chain to in an environment of the robot via anobject-coordinate frame of an object in the environment.
 10. The methodfor controlling at least one effector trajectory according to claim 1,wherein each posture of the acquired sequence of postures is independentof other postures of the acquired sequence of postures.
 11. The methodfor controlling at least one effector trajectory according to claim 1,wherein the method for controlling the at least one effector trajectorycontrols the effector in real-time.
 12. The method for controlling atleast one effector trajectory according to claim 1 further comprises:adjusting the effector trajectory of the robot to match a qualityfunction, wherein the quality function includes a description of a humanergonomic state determined based on a given ergonomic model of a humanwhich collaborates with the robot, and wherein the adjusted effectortrajectory defines an effector-and-object pose of the robot.
 13. Themethod for controlling at least one effector trajectory according toclaim 1 further comprises: determining an optimized location of therobot for performing the task.
 14. The method for controlling at leastone effector trajectory according to claim 1, wherein the method isperformed for solving the predetermined task with each of at least twodifferent robots, and wherein the method further comprises: determiningwhich of the at least two different robots is more suitable by comparingquality criteria for performing the task by each of the at least twodifferent robots.
 15. The method for controlling at least one effectortrajectory according to claim 1, wherein in the step of performing theconstraint relaxation on the generated set of constraint equations, atleast a constraint equation on at least one location of a tool or objectrequired for solving the task is relaxed, and wherein the method furthercomprises: determining at least one optimized location of the tool orobject for designing a workplace within an environment of the robot. 16.The method for controlling at least one effector trajectory according toclaim 1 further comprising: acquiring a plurality of sequences ofpostures for performing the predefined task, wherein individualsequences of postures of the acquired plurality of sequences of posturesdiffer by the number of individual postures, performing the method foreach individual sequence of postures of the acquired plurality ofsequences of postures, and discarding all individual sequences ofpostures of the acquired plurality of sequences of postures which resultin a corresponding effector trajectory that violates at least one motionlimit of the robot.
 17. A non-transitory computer-readable storagemedium embodying a program of machine-readable instructions executableby a digital processing apparatus to cause the digital processingapparatus to perform the method according to claim
 1. 18. A roboticsystem for controlling at least one effector trajectory, of a robot forsolving a predefined task, the robotic system comprising: an acquisitionunit configured to acquire a sequence of postures, each postureincluding at least one contact point and a kinematic pose of the atleast one effector; a processor configured to: modify at least one of acontact constraint topology according to the acquired sequence ofpostures, and an object constraint topology according to the acquiredsequence of postures, generate a set of constraint equations based on atleast one of the modified contact constraint topology and the modifiedobject constraint topology, perform constraint relaxation to generate aset of relaxed constraint equations to generate a task descriptionincluding the set of relaxed constraint equations, perform an inversekinematics algorithm on the generated at least one effector trajectoryfor generating a control signal for controlling the at least oneeffector; and output the control signal to the robot; and the robotconfigured to control the at least one effector based on the controlsignal.